#include "pid.h"
#include "stdio.h"

float error_x=0,ierror_x=0,derror_x=0,errorlast_x=0,error_y=0,ierror_y=0,derror_y=0,errorlast_y=0;
//float error_gx=0,ierror_gx=0,derror_gx=0,errorlast_gx=0,error_gy=0,ierror_gy=0,derror_gy=0,errorlast_gy=0;
float kp_x = 1.2  ,kp_y = 1.2           ,kp_gx = 0.09    ,kp_gy = 0.09;
float ki_x = 0       ,ki_y = 0              ,ki_gx = 0       ,ki_gy = 0;
float kd_x = 0.3  ,kd_y = 0.3            ,kd_gx = 0       ,kd_gy = 0;
float dia_x=0        , dia_y=0              ,dia_gx=0        , dia_gy=0; 
float angle_x=1420 ,angle_y=1470               ,angle_gx=90     ,angle_gy=90;
float anglex = 0,angley = 0;
int cha=1;
int x_middle=150,y_middle=100;//147,138
//#define x_middle 150 //142
//#define y_middle 100 //131

//#define gx_middle 80
//#define gy_middle 60
void limit (float *new_angle_x)
{
	if(*new_angle_x>1600){*new_angle_x=1600;}
	else if(*new_angle_x<1360){*new_angle_x=1360;}
}
void limit1 (float *new_angle_y)
{
	if(*new_angle_y>1755){*new_angle_y=1765;}
	else if(*new_angle_y<1455){*new_angle_y=1455;}
}
void pid_red_x(int x)  
{
	error_x =x_middle- x*1.0;//  30  -3  33
	ierror_x=ierror_x+error_x;
	derror_x=error_x-errorlast_x;
	errorlast_x=error_x;
	dia_x = kp_x*error_x+kd_x*derror_x+ki_x*ierror_x;//
	anglex = angle_x +dia_x;
	limit(&anglex);
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, anglex);//x
}

void pid_red_y(int y)
{
	error_y = y_middle-y*1.0;
	ierror_y=ierror_y+error_y;
	derror_y=error_y-errorlast_y;
	errorlast_y=error_y;
	dia_y=kp_y*error_y+kd_y*derror_y+ki_y*ierror_y;
	angley = angle_y - dia_y;
	limit1(&angley);
	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2,angley);
}
void pid_stop(void)
{
	if((val_x>x_middle+cha || val_x<x_middle-cha) || (val_y>y_middle+cha || val_y<y_middle-cha))
	{
		flag_run=0;
	}
	else if((val_x<x_middle+cha && val_x>x_middle-cha) && (val_y<y_middle+cha && val_y>y_middle-cha))
	{
		flag_run=1;
	}
}

//void pid_base4_y(int y)
//{
//	error_y = y_middle-y*1.0;
//	ierror_y=ierror_y+error_y;
//	derror_y=error_y-errorlast_y;
//	errorlast_y=error_y;
//	dia_y=kp_y*error_y+kd_y*derror_y+ki_y*ierror_y;
//	angley = angle_y - dia_y;
//	limit1(&angley);
//	__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2,angley);
//}

